#include <cmath>
#include <vector>
#include <string>
#include "aloam/common.h"
#include "aloam/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

using std::atan2;
using std::cos;
using std::sin;

const double scanPeriod = 0.1;
const int systemDelay = 0;
int systemInitCount = 0;
bool systemInited = false;
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];

bool comp(int i, int j) { return (cloudCurvature[i] < cloudCurvature[j]); }

ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubRemovePoints;
std::vector<ros::Publisher> pubEachScan;

bool PUB_EACH_LINE = false;
double MINIMUM_RANGE = 0.1;

////////////////////// 剔除距离雷达太近的点 //////////////////////
// template函数模板; PointT 是PCL中基本的点的表示形式
template<typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                            pcl::PointCloud<PointT> &cloud_out, float thres)
{
    // 统一header(时间戳)和size
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;                 // header同步
        cloud_out.points.resize(cloud_in.points.size());    // 容器大小同步
    }
    // size_t一般用来表示一种计数
    size_t j = 0;
    // 逐点比较欧式距离，把点云中距离小于给定阈值thres的点去除掉，把cloud_in 放入cloud_out
    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
        if (pow(cloud_in.points[i].x, 2) + pow(cloud_in.points[i].y, 2) +
            pow(cloud_in.points[i].z, 2) < thres * thres) continue;
        cloud_out.points[j] = cloud_in.points[i];
        // 有点被剔除时，size改变
        j++;
    }
    // 调整容器的长度大小，使其能容纳j个元素。因为，有的i没有放到cloud_out中，i<j的
    if (j != cloud_in.points.size()) cloud_out.points.resize(j);
    // 这里是对每条扫描线上的点云进行直通滤波，因此设置点云的高度为1，宽度为数量，稠密点云
    cloud_out.height = 1;
    // 可以理解成点数，宽度
    cloud_out.width = static_cast<uint32_t>(j);
    // 指定点中的所有数据是有限的（true)
    cloud_out.is_dense = true;
}

////////////////////// 处理雷达点云 //////////////////////
// 对接收到的点云进行排序，求曲率，根据曲率分类，体速滤波降采样，发送topic
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    // 作用就是延时，为了确保有IMU数据后, 再进行点云数据的处理
    if (!systemInited)
    {
        systemInitCount++;
        // 初始化的次数大于等待时间 进行初始化
        if (systemInitCount >= systemDelay) systemInited = true;
        else return;
    }
    TicToc t_whole;             // 计时：整个回调函数的时间
    TicToc t_prepare;           // 计时：雷达点云有序化的时间
    // 每条雷达扫描线上（scan）的可以计算曲率的点云点的起始索引和结束索引，分别用scanStartInd数组和scanEndInd数组记录
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;        // 定义一个pcl形式的输入点云
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);      // 把ros包的laserCloudMsg点云转化laserCloudIn为pcl形式
    std::vector<int> indices;
    // 以下，使用 pcl::removeNaNFromPointCloud 和 removeClosedPointCloud 的 cloud_in 和 cloud_out都是 laserCloudIn
    // 也就是，输入laserCloudIn，处理之后，再将输出定为laserCloudIn，
    // 所有，没有RemovePoints， 相关publisher也没用
    // pcl库函数移除无效点，pcl库函数
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    // 移除过近点，自定义子函数
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
    // 下面要计算点云角度范围，是为了使点云有序，需要做到两件事：
    // 1、为每个点找到它所对应的扫描线（SCAN）
    // 2、为每条扫描线上的点分配时间戳。
    // 要计算每个点的时间戳，首先我们需要确定这个点的角度范围。可以使用<cmath>中的atan2( )函数计算点云点的水平角度。
    int cloudSize = laserCloudIn.points.size();        
    // 每次扫描是一条线，看作者的数据集激光x向前，y向左，那么下面就是线一端到另一端
    // atan2的输出为-pi到pi(PS:atan输出为-pi/2到pi/2),
    // atan2()函数是atan(y， x)函数的增强版，不仅可以求取arctran(y/x)还能够确定象限
    // 计算旋转角时取负号是因为velodyne是顺时针旋转，速腾顺时针旋转，Y朝前
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    // 这里加上2PI是为了将atan2的范围转换到[0, 2π)
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;  
    // 激光间距 1pi到3pi
    if (endOri - startOri > 3 * M_PI) endOri -= 2 * M_PI;
    else if (endOri - startOri < M_PI) endOri += 2 * M_PI;
    // printf("end Ori %f\n", endOri);
    // 点云点找到对应的扫描线，每条扫描线都有它固定的俯仰角，我们可以根据点云点的垂直角度为其寻找对应的扫描线。
    // 过半记录标志
    bool halfPassed = false;
    // 记录总点数
    int count = cloudSize;
    PointType point;
    // 按线数保存的点云集合
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
    // 循环对每个点进行以下操作
    ////////////////////// 1、为每个点找到它所对应的扫描线（SCAN） //////////////////////
    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;
        // 求仰角atan输出为-pi/2到pi/2，实际看scanID应该每条线之间差距是2度
        float angle = rad2deg(atan(point.z / sqrt(pow(point.x, 2) + pow(point.y, 2))));
        // 线数ID
        int scanID = 0;         
        // 根据不同线数使用不同参数对每个点对应的第几根激光线进行判断
        // 如果是16线激光雷达，结算出的angle应该在-15~15之间，+-15°的垂直视场，垂直角度分辨率2°，则-15°时的scanID = 0。
        if (N_SCANS == 16)
        {
            // int舍尾取整，+0.5是误差限，最接近的线数，但不是四舍五入
            scanID = int((angle + 15) / 2 + 0.5);
            // 如果判定线在16以上或是负数则过滤该点,回到上面for循环
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        // 32线雷达， 垂直视场角+10.67~-30.67°，垂直角度分辨率1.33°，-30.67°时的scanID = 0
        else if (N_SCANS == 32)
        {
            scanID = int((angle + 92.0 / 3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {
            if (angle >= -8.83) scanID = int((2 - angle) * 3.0 + 0.5);
            else scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
            // use [0 50]  > 50 remove outlies
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            ROS_BREAK();
        }
        // printf("angle %f scanID %d \n", angle, scanID);
        // 计算水平角
        float ori = -atan2(point.y, point.x);
        // 根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿
        if (!halfPassed)
        {
            // 确保 -pi / 2 < ori - startOri < 3 * pi / 2   为什么？
            if (ori < startOri - M_PI / 2) ori += 2 * M_PI;
            else if (ori > startOri + M_PI * 3 / 2) ori -= 2 * M_PI;
            // 扫描点过半则设定halfPassed为true，如果超过180度，就说明过了一半了
            if (ori - startOri > M_PI) halfPassed = true;
        }
        // 确保 -3 * pi / 2 < ori - endOri < pi / 2
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2) ori += 2 * M_PI;
            else if (ori > endOri + M_PI / 2) ori -= 2 * M_PI;
        }
        // 计算旋转角占总角度的比值 即点云中的相对时间
        float relTime = (ori - startOri) / (endOri - startOri);
        // scanID为线号， 小数点后面为相对时间，放入point信息
        point.intensity = scanID + scanPeriod * relTime;
        // 按线把点存好
        laserCloudScans[scanID].push_back(point);
    }
    // cloudSize是有效的点云的数目
    cloudSize = count;
    printf("points size %d \n", cloudSize);
    ////////////////////// 2、曲率计算 //////////////////////
    // 把所有线保存在laserCloud一个数据集合里，
    // 把每条线的第五个和倒数第五个位置反馈给scanStartInd和scanEndInd
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    {
        // 处理每个scan 每条scan上面的 点的起始 id 前5个点不要（为了后面计算曲率） 结束的id 为后6个点不要（其中一个不参与计算）
        // 开始和结束处的点云容易产生步闭合的"接缝"，对提取edge_feature不利
        // 整理后重新放入lasercloud
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;
    }
    // 预处理部分完成，记录一下时间
    printf("prepare time %f \n", t_prepare.toc());
    // 曲率公式
    // 由于计算中我们只是为了比较曲率大小，所以只计算了前后五个点与当前点的差，然后求其平方代替曲率计算
    for (int i = 5; i < cloudSize - 5; i++)
    {
        // 使用每个点的前、后五个点共11个点计算曲率
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x +
                      laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x +
                      laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x +
                      laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y +
                      laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y +
                      laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y +
                      laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z +
                      laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z +
                      laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z +
                      laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
        // 计算曲率 求平方
        cloudCurvature[i] = pow(diffX, 2) + pow(diffY, 2) + pow(diffZ, 2);
        // 记录曲率索引，一轮循环后变为cloudSize - 6
        cloudSortInd[i] = i;
        // 标记是否筛选过
        cloudNeighborPicked[i] = 0;
        // 标记点类型
        cloudLabel[i] = 0;
    }
    // 计时
    TicToc t_pts;
    // 定义四种特征点
    pcl::PointCloud<PointType> cornerPointsSharp;           // 曲率很大的特征点    对应2
    pcl::PointCloud<PointType> cornerPointsLessSharp;       // 曲率稍微大的特征点  对应1
    pcl::PointCloud<PointType> surfPointsFlat;              // 曲率很小的特征点    对应-1
    pcl::PointCloud<PointType> surfPointsLessFlat;          // 曲率稍微小的特征点  对应0
    // 用来记录排序花费的总时间
    float t_q_sort = 0;
    // 遍历所有scan
    for (int i = 0; i < N_SCANS; i++)
    {
        // 如果该scan的点数小于6个点，就不能分为6个扇区，跳过
        if (scanEndInd[i] - scanStartInd[i] < 6) continue;
        // 定义surfPointsLessFlatScan储存该扫描线上的less_flat点云
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        // 对每个扫描进行6等分
        for (int j = 0; j < 6; j++)
        {
            // 起始序号
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
            // 结束序号    -1是避免提取到相同的点
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
            // 计时，排序sort时间
            TicToc t_tmp;
            // 对cloudSortInd进行排序   依据的是序号cloudSortInd[i]的点的曲率从小到大
            // sort（数组的起始位置，数组的结束位置， 比较方法）
            std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);
            t_q_sort += t_tmp.toc();
            // 记录曲率大点的数目
            int largestPickedNum = 0;
            // 挑选各个分段曲率很大和比较大的点
            for (int k = ep; k >= sp; k--)
            {
                // 曲率最大点的序号
                int ind = cloudSortInd[k];
                // 没被选过 && 曲率 > 0.1
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
                {
                    largestPickedNum++;
                    // 该subscan中曲率最大的前2个点认为是Corner_sharp特征点
                    if (largestPickedNum <= 2)
                    {
                        // Label 2：corner_sharp
                        cloudLabel[ind] = 2;
                        // 选取该点为极大边缘点
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        // 也将这两个corner_sharp点认为是次极大边缘点，包括上一种
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    // 3～20这18个点
                    else if (largestPickedNum <= 20)
                    {
                        // Label 1：corner_less_sharp
                        cloudLabel[ind] = 1;
                        // 只属于cornerPointsLessSharp
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    } else break;
                    // 筛选过的点标志为 1 不进行下次的计算
                    cloudNeighborPicked[ind] = 1;
                    // 考虑该特征点序号前后5个点，如果有两两之间距离过于接近的点
                    // 则不考虑将其前一个作为下一个特征点的候选点  这是防止特征点聚集，使得特征点在每个方向上尽量分布均匀
                    for (int l = 1; l <= 5; l++)
                    {
                        // 提取该点 后 五个点的xyz方向的导数
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) break;
                        // 距离平方小于0.05的，标记为计算过
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        // 提取该点 前 五个点的xyz方向的导数
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) break;
                        // 计算过的点标志 1
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }
            // 提取surf_flat的feature，选择该subscan曲率最小的前4个点为surf_flat
            int smallestPickedNum = 0;
            // 从曲率小的点开始提取surf_flat特征，此时sp到ep内对应的曲率由小到大
            for (int k = sp; k <= ep; k++)
            {
                // ind对应曲率最小的点的索引
                int ind = cloudSortInd[k];
                // 如果ind对应的点未被选中，且曲率小于0.1
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1)
                {
                    cloudLabel[ind] = -1;
                    surfPointsFlat.push_back(laserCloud->points[ind]);
                    smallestPickedNum++;
                    if (smallestPickedNum >= 4) break;
                    // 避免每个特征点过于集中，将选中的点的周围5个点都置1，避免后续会选到
                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // 目前选出了曲率最大的前20个点认为是corner_less_sharp(其中前两个为corner_sharp)，选出了曲率最小的4个点认为是surf_flat
            // 其他的非corner特征点与surf_flat特征点共同组成surf_less_flat特征点
            // 最初将所有点都标记为0了，初始化的时候，见  cloudLabel[i] = 0;
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

        // 由于less flat点最多，对每个分段less flat的点进行体素网格滤波，降采样
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;        // 定义体素滤波后的点云集合名称
        pcl::VoxelGrid<PointType> downSizeFilter;                   // 定义体素滤波
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);       // surfPointsLessFlatScan中存着surf_less_flat特征点
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);                  // 设置体素大小
        downSizeFilter.filter(surfPointsLessFlatScanDS);            // 执行滤波
        // less flat点汇总  surfPointsLessFlatScanDS是滤波中间集合，滤波后又赋值回surfPointsLessFlat，label = 0
        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }
    printf("sort q time %f \n", t_q_sort);                          // 打印排序时间
    printf("seperate points time %f \n", t_pts.toc());              // 打印点云分类时间
    // 发布点云信息  laserCloud
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    laserCloudOutMsg.header.frame_id = "camera_init";
    pubLaserCloud.publish(laserCloudOutMsg);
    // 发布曲率大的点云信息 即cornerPointsSharp  label = 2
    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);
    // 发布曲率一般大的点云信息 即cornerPointsLessSharp label = 1
    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
    // 发布曲率小的点云信息 即surfPointsFlat  label = -1
    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);
    // 发布曲率较小的点云信息 即surfPointsLessFlat  label = 0, 降采样后的常规点云
    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
    // laserCloudIn，试验pubRemovePoints
    pubRemovePoints.publish(surfPointsLessFlat2);
    // pub each scam
    // 发布每一条线的信息 由一开始进行设置，默认不发送，false
    if (PUB_EACH_LINE)
    {
        for (int i = 0; i < N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            // laserCloudScans按线数保存的集合，无序，但是划分线数
            pcl::toROSMsg(laserCloudScans[i], scanMsg);
            scanMsg.header.stamp = laserCloudMsg->header.stamp;
            scanMsg.header.frame_id = "camera_init";
            pubEachScan[i].publish(scanMsg);
        }
    }
    // 特征提取总用时
    printf("scan registration time %f ms *************\n", t_whole.toc());
    // 处理超时
    if (t_whole.toc() > 100) ROS_WARN("scan registration process over 100ms");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");
    ros::NodeHandle nh;
    // param()函数从参数服务器取参数值给变量。如果无法获取，则将默认值赋给变量。
    // 设置激光雷达的扫描线数量
    nh.param<int>("scan_line", N_SCANS, 16);
    // 点与激光雷达的最近距离
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
    printf("scan line number %d \n", N_SCANS);
    // 如果雷达的线数不对结束运行
    if (N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
    {
        printf("only support velodyne with 16, 32 or 64 scan line!");
        return 0;
    }
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("velodyne_points", 100, laserCloudHandler);
    // 发布点云信息  以下六类点
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("velodyne_cloud_2", 100);
    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_sharp", 100);
    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_less_sharp", 100);
    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_flat", 100);
    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_less_flat", 100);
    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("laser_remove_points", 100);
    // 检测每条发布的激光数据，并注明id存好
    if (PUB_EACH_LINE)
    {
        for (int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("laser_scanid_" + std::to_string(i), 100);
            pubEachScan.push_back(tmp);
        }
    }
    ros::spin();
    return 0;
}
